/*
 * Copyright (C) 2015-2017 Alibaba Group Holding Limited
 */

#include "can_demo.h"
#include <stdio.h>

#include "aos/hal/can.h"
#include "board.h"

#include "ulog/ulog.h"

#define CAN_RX_BUFFER_SIZE 16
static can_frame_t can_rx_buf[CAN_RX_BUFFER_SIZE];
static aos_queue_t can_rx_queue;

static can_dev_t can_dev_instanse = {PORT_CAN_RESERVED, {250000, 0, 0, 0}, NULL};

static void canio_rx_isr(can_frame_t *can_frame_rx, void* para);

/* can filter list for master */
static can_filter_item_t can_filter_items[] =
{
    {0, 0x80, 0x780},
    {0, 0x180, 0x780},
    {0, 0x280, 0x780},
    {0, 0x380, 0x780},
    {0, 0x480, 0x780},
    {0, 0x580, 0x780},
    {0, 0x700, 0x780},
};

#ifdef LOG_TAG
#undef LOG_TAG
#endif

#define LOG_TAG "CAN_DEMO"

static void canio_rx_isr(can_frame_t *can_frame_rx, void* para)
{
    if (NULL != can_frame_rx && NULL != para) {
        aos_queue_t* queue = (aos_queue_t*)para;
        aos_queue_send(queue, can_frame_rx, sizeof(can_frame_t)-(8-can_frame_rx->header.dlc));
    }
}

static int32_t normal_can_rx(can_frameheader_t *rxheader, void *data, const uint32_t timeout)
{
    int32_t rc = -1;
    can_frame_t can_frame;
    int32_t size = 0;
    rc = aos_queue_recv(&can_rx_queue, timeout, &can_frame, &size);
    if (0 == rc) {
        memcpy(rxheader, &can_frame.header, sizeof(can_frameheader_t));
        if (can_frame.header.dlc > 0 && can_frame.header.rtr == 0 && data != NULL) {
            memcpy(data, &can_frame.data, can_frame.header.dlc);
        }
    }
    return rc;
}

static int lidar_status = 0;
static void can_demo_routine(void *param)
{
    can_frameheader_t header;
    uint8_t data[8];
	int i = 0;
    while(1) {
        if(0 == hal_can_recv(&can_dev_instanse, &header, data, AOS_WAIT_FOREVER) ) {
            can_frame_t tx_frame;
            LOGI(LOG_TAG, "Rx CAN ID 0x%x(%d), RTR:%d",header.id, header.dlc, header.rtr);
            if (header.id == 0x587 && lidar_status != (data[0] & 0x1)) {
				lidar_status = (data[0] & 0x1);
				lidar_heartbeat_handle(data);
			}
			else if (header.id == 0x586) {
				lidar_data_handle(data);
			}
			else if (header.id == 0x88) {
				for (i = 0; i < header.dlc; i++) {
					printf("0x%x", data[i]);
					//lidar_tf_data_tx(&data[i], 1);
				}
				printf("\n");
				//lidar_data_handle(data);
			}

#if 0

			tx_frame.header.id = header.id+0x80;
            tx_frame.header.dlc = header.dlc;
            tx_frame.header.rtr = header.rtr;
            memcpy(tx_frame.data, data, header.dlc<8?header.dlc:8);
			
            if(0 != can_send(&tx_frame)) {
                LOGE(LOG_TAG, "Tx CAN Failed");
            }
#endif

        }
    }
}

int32_t can_init(void)
{
    int32_t rc = -1;
    if (0 == aos_queue_new(&can_rx_queue, can_rx_buf, sizeof(can_rx_buf), CAN_RX_BUFFER_SIZE)) {
        if (0 != hal_can_init(&can_dev_instanse)) {
            LOGE(LOG_TAG, "Init can Fail");
        } else {
            can_mapping_t *can_mapping = can_mapping_node(can_dev_instanse.port);

            if (NULL != can_mapping) {
                can_mapping->isr_rx_handler = canio_rx_isr;
                can_mapping->customer_recv  = normal_can_rx;
                can_mapping->isr_rx_para    = &can_rx_queue;

                if (0 != hal_can_filter_init(&can_dev_instanse, sizeof(can_filter_items) / sizeof(can_filter_items[0]), can_filter_items)) {
                    LOGE(LOG_TAG, "Set CAN Filter Failed");
                } else {
                    if (0 != hal_can_start(&can_dev_instanse)) {
                        LOGE(LOG_TAG, "Start CAN Failed");
                    } else {
                        LOGI(LOG_TAG, "Start can sucessfully");
                        if (0 != aos_task_new("can_demo", can_demo_routine, NULL, CAN_DEMO_TASK_DEPTH)) {
                            LOGE(LOG_TAG, "create routine fail");
                        }
                        rc = 0;
                    }
                }
            } else {
                LOGE(LOG_TAG, "Over Write can Fail");
            }
        }
    } else {
        LOGE(LOG_TAG, "Init CAN Rx MessageQ Failed");
    }

    return rc;
}

int32_t can_send(const can_frame_t *frame)
{
    int32_t rc = -1;
    if (NULL != frame) {
        rc = hal_can_send(&can_dev_instanse, &frame->header, frame->data, CAN_SEND_BLOCK_MS);
    } else {
        LOGE(LOG_TAG, "invalid param");
    }

    return rc;
}
